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ABSTRACT 


The Kalman Filter is implemented to process range data output from 
the Cubic Model 40 Autotape system, a surface position locating system 
currently employed on the underwater tracking ranges at Dabob Bay and 
Nanoose. Results are presented for different measurement noise and 


forcing function noise statistics. 
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IT. INTRODUCTION 


The Cubic CM-40 Autotape is a microwave distance measuring system 
used (by the U.S. Navy at its acoustic underwater tracking ranges at 
Dabob Bay and Nanoose) to provide reference position information for 
units on the surface and in the air above the range. This portable 
system consists basically of an interrogator which is operated aboard 
the unit to be tracked, two responders operated at two different shore 
Sites and the associated antenna/RF assemblies. Required support systems 
include a data display and recording setup and an ADP facility for off- 
line processing of the Autotape data. Figure 1 shows the Autotape system 
components and Figure 2 shows a typical application geometry. 

Historically, the Autotape has been used in such applications as 
tracking hydrophone array survey, buoy and hydrophone array planting and 
as a reference position indicator for calibrating other position-finding 
devices against. Generally, the Autotape has been used where an extremely 
high degree of accuracy is not required. 

In operation, the system will provide for the display and recording 
of two ranges simultaneously, once per second, the ranges being those 
between the anteCrogater and each of the responders. The ranges are 
computed from the phase delay between the output of the modulation signal 
generator and a signal which has traveled from the interrogator to a 
responder and back. Ranging accuracy is stated by the manufacturer to 
be * 0.5 meter + 10 ppm x range. Ranging frequencies of 1500 KHZ, 150 
KHZ and 165 KHZ modulate a 3000 MHZ carrier, yielding a maximum unambiguous 


range of 10,000 meters with a resolution of 0.1 meter. However, independent 
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FIGURE 1: Cubic Model 40 Autotape System 
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testing by the U.S. Navy [Reference 1] has shown that system accuracy may 
not be quite as good as stated by the manufacturer. 

The accuracy of the Autotape system is principally dependent upon 
range errors, the geometry of the system and the method of data reduction. 
These factors are, in turn, affected by propagation velocity, system 
stability, range dependency, land survey accuracy, system geometry, 

Slope reduction and data smoothing. A final anomaly which, depending 

upon the application, can substantially degrade the quality of the data- 
stream out is the orientation, over time, of the interrogator antenna in 
the vertical dimension. The interrogator antenna has only a 10 degree 
vertical beam width. Thus, if the system is being used on a platform 

such as a moderately maneuvering helicopter or a ship rolling substan- 
tially in the seaway, the system tends to frequently lose track, resulting 
in fairly long streams of useless data. 

Present data reduction techniques employed when the system is used 
on either of the ranges (Dabob or Nanoose) employ two overall iterations. 
The first, or initial processing, administers the following three correc- 
tions to the raw range data: 


1. Range Calibration Correction: This is a fixed value (meters) added 
to or subtracted from each range. 


2. Propagation Velocity Correction: This is a variable correction due 
to the atmospheric index of refraction at the particular time and 
place of the exercise. 


3. Slope Reduction Correction: This reduces both range measurements 
(which are actually slant ranges because the interrogator and the 
responders are not normally located at the exact same elevation) 
to a common horizontal plane at sea level. 





Subsequent processing of the data includes conversion of the corrected 
ranges to a rectangular x-y range coordinate system and a moving average 


smoothing technique which employes curve fitting algorithms (linear, 
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parabolic or logarithmic) to reduce the data to its final form. Not 
uncommonly, as a result of the total reduction effort, the net remainder 
is an inadequate data package (in terms of quantity) for proper final 
evaluation. 

Figure 3 is a rectangular plot of the raw ranges recorded during a 
recent array survey. The purpose of this project has been to design a 
filter, a Kalman filter, which would provide more accurate range data, 
as well as one that would track through the periods of “lost track" 
ranging, thereby providing a significantly larger final volume of data 
for evaluation. This paper presents the basic theory necessary and 


includes the final version of the filter. 
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IT. THE FILTER THEORY AND DESIGN 


A. THE SYSTEM DYNAMIC MODEL 

A common application for the Autotape system is its use as a reference 
position locator on the surface unit conducting an acoustic hydrophone 
array (range) survey. The usual exercise plan will call for a service 
unit, carrying the interrogator and equipped with an acoustic pinger 
mounted on the underwater hull, to transit three concentric circular 
tracks, centered above the array, with track radii ranging from 100 to 
1,000 meters, at speeds of up to eight knots. The direction of rotation 
for the outer track will normally be opposite to that of the middle 
circle. While the service unit is being tracked via Autotape, it is also 
being tracked by the acoustic array. By comparing the acoustic position 
data with that from the Autotape, a digital computer is able to compute 
actual position and attitude of the array. 

The desired estimates will be those of position and velocity, Rl, R2, 
R1, R2. It is proper at this point to define a number of terms and to 
Summarize some pertinent results of observer theory. First, we may define 


a fourth order state vector: 


Recall that a linear system can be described in the continuous time 


domain as: 


i2 





where: x(t) is the n-element column vector of the states 
A and D are nxn and nxp matrices describing system dynamics 


w(t) is a q-element vector of random noise inputs to the 
system 


The system measurements may be expressed as: 


Zee XG) vy) 


where: z(t) is the q-element vector of system measurements 
H is the qxn weighting matrix for the measurements 


v(t) is the q-element vector of random measurement noise 


The corresponding linear discrete model may be written as: 
x(k + 1) = @ x(k) + £ w(k) 


with no deterministic inputs to the system. 


Also, z(k) = H x(k) + v(k) 


For the system under consideration, it can be shown that the state 


transition matrix 


1 OO 
Q = 05 1° Ot 
0010 
00°04) 
0 
Q % 
and i 10 
0 1 


for a sampling interval T of 1 second. A block diagram of the system is 


shown in Figure 4. 
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FIGURE 4: 


Block Diagram of Discrete Linear Estimator 
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The following assumptions will be made regarding the noise processes 
and the initial state, x(o) of the plant [Ref. 2]: 
The measurement noise has zero mean, is uncorrelated, and 


Ea v(k) v'(j)] = R(k) 3. ;, where $ is the kronecker delta 


The forcing noise has zero mean, is uncorrelated, and 


E [ w(k) w'(5)] = Q(k) 8, 


The forcing noise and measurement noise are uncorrelated. 


The initial state is a random variable with known mean and covariance, 
and 
EL {x(o) - x} {x(o) - x} 1] =P 
The measurement noise and initial state are uncorrelated. 
The forcing noise and initial state are uncorrelated. 


The Kalman Filter equations and their derivation are well! known 


[Ref. 2], [Ref. 3]: 


G(k) = P(k/k-1) H'(k) [H(k) P(k/k-1) H'(k) + R(k)]7+ (1) 
P(k/k-1) = @ P(k-1/k-1) g' + Q or (2) 
P(k/k) = [L - G(k) H(k)] P(k/k-1) Og 
“S X(k/k) = x(k/k-1) + G(k) [z(k) - H(k) x(k/k-1)] (4) 
X(k/k-1) = O(k/k-1) x(k-1/k-1) + P(k/k-1) w(k-1) (5) 


Where the notation (k/k-1) interprets as the value of the parameter of 
note at time k given measurements at times up to and including time k-1l. 
(k/k) and (k-1/k-1) have similar interpretations. The X denotes the 


estimate of x. 


ND 





G(k) represents the filter gain at time k. P represents the covariance 


of estimation error; 


e, (k/k) 
i e5(k/k) 
P (k/k) = E [e(k/k) e'(k/k)] = E 4], [ey (k/k) @5(k/k)-=-e, (k/K)} 
e,(k/k) 
ef (k/k) e,(k/k) e5(k/k) === @,(k/k) @,(k/k) 
eo (k/k) e,(k/k) —e5(k/k) --- ey(K/k) @(K/k) 
=ar 
e(k/k) e,(k/k) @,(k/k) e5(k/k) === e°(k/k) 


where e(k/k) = X(k/k) - x(k). A complete standard block diagram for the 
filter and an information flow diagram are included as Figures 5 and 6 
as slightly different viewpoints from which the system may be viewed and 
understood. Figure 7 shows a timing diagram of the various quantities 


contained in the filter equations. 
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H(k-1) R(k-1) Q(k-1) 6(k-1) 






Compute 





Compute 





“P(k/k-1) = @ P(k-1/k-1)9"+ 0 





P(k/k) = (1-G(k)H(k)] P(k/k-1) 


H(k) Compute 


R(k) | G H(k)P(k/k-1)H'(k) + R(k)17 





G(k) —w(k-1)) @(k-1)  2(k-1) 


Compute 


X(k/k) = x(k/k-1)+G(k) set k 


x(k/k-1) = Ox(k-1/k-1) 
[z(k)-H(k)x(k/k-1)] 





z(k) x(k/k-1) 
Xe 


FIGURE 6: Simplified Information Flow Diagram of a Discrete Kalman Filter 
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B. THE PROCESSOR 

Appendix A is a flowchart of the Kalman filter program utilized. 
Initially, the matrices describing the physical system, the noise 
statistics and other program parameters are read into storage and printed 
out. The discrete state-transition matrix, Phi, 1s computed and printed 
out and the gain schedule is computed and printed out. It is seen that 
the elements of the gain matrix reach a steady state, and, for example, 
with both the R and Q matrices being identity matrices, the gain reaches 
steady state between k=5 and k=10. Therefore, in the main iteration loop, 
the filter will essentially be a constant gain filter for k> 10. 

Next, the main iteration loop commences. The initial measurements 
are read and x1(0/-1) and x2(0/-1) are initialized to these values. 
x3(0/-1) and x4(0/-1), representing the rates, are set to the mean 
constant value (in the respective directions) of 4.0 meters per second. 
The Autotape output is a 5 significant figure output, modulo 10,000, 
reading to 0.1 meter. Inherent in the output is a major degree of jitter 
in the two most significant digits, which would significantly distort 
the covariance of measurement noise. Therefore, as an option, measure- 
ments could be gated, and the gain automatically set to zero in those 
cases where the residue falls outside of a maximum reasonable bound. 

Commencing with k=0, and utilizing the known values for x(0/-1) and 
P(0/-1), the Kalman filter equations are solved iteratively in the 
following manner [see page 15, equations (1)-(5)]: 

(1), (3), (4), 
Increment k to k=l 

(5), (2), (1), (3), (4), 
Increment k to k=2 


(5), (2), (1), (3), (4), 


BL. 
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Also computed on each iteration are the error residues: 
RES = Z - x(k/k-1) 

and the one-step prediction errors: 
ERR = x(k/k) - x(k/k-1) 


Finally, the computations are tabulated and plots are produced. 


C. NOISE AND ERROR CONSIDERATIONS 

Reference 1 documents an Autotape evaluation which was conducted in 
1971. The error geometry is shown in Figure 8. Graphically, position 
1s determined by locating the crossing point of the two range arcs, in 
conjunction with a knowledge of the baseline formed by the two responders. 
Since each range has an associated standard deviation (error), the point 
can actually be enclosed in a parallelogram which defines the probable 
position within one standard deviation of the ranges. The shape of the 
parallelogram will vary with the position of the crossing point relative 
to the baseline, as indicated in Figure 8. It can be shown that the 
maximum probable error (MPE) will be minimized where the range arcs are 
orthogonal. Figure 9 diagrams error contours which are actually the 
locii of constant MPE for two particular responder sites on the Nanoose 


Range. Table 1 summarizes pertinent results of the study. 
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TABLE 1 


Average Range Errors (feet) 


Survey 


Array 04 
Array 07 
Array 08 
Array 09 


Average 


No. Points 


30 
49 
10 
25 












Error Standard 
Average Deviation 


Error Standard 
Average Deviation 












= 055 2.8 - 0.1 
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\ JS 
For the purpose of modeling the covariance of excitation noise, it 


was assumed that the service unit transited an 800 meter circle at an 


average speed of eight knots. Then: 





800 meters 


=: 0207 aa 
sec 
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Filter performance was investigated for Q=I1, .11, and .Oll, 


fOr 
G Ln ecw 


= a = = | ORE Ure 
*o i fe ; ¥, ao - ef J O70 a iaG 


0001 


P(0/-1) 


1 0 
and R = 01 


where the a priori x(0/-1) is known to be a reasonably good estimate -- 


approximately the same accuracy as an observation. 


D. PROCESSOR PERFORMANCE; AUTHOR'S CONCLUSIONS 

Table 2 summarizes a comparison of the Kalman filter performance with 
the results of the (corrected) processing by the program presently being 
used for the cases Q=1, R=I1, andQ=0.11, R=I1. Figures 10, 11, 

12 and 13 are residue and error plots for the example Q = .O1I, R = I. 

It is seen that the Kalman filter will satisfactorily handle the data 
where the measurement noise statistics approximate those used in the 
model. However, for the noise resulting from the jitter which appears 
in the "hundreds" and "thousands" digits, the filter, as configured 


without a gate, will estimate with considerable error. The raw range 


Zo 


R2 was clean of this particular noise element, and the results as 
indicated by Figures 12 and 13 were superior to those for R1. 
It is suggested that the Kalman filter be used as the first iteration 


processing of the Autotape output. 
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FIGURE 11: Residue 2 vs. Time. Q= .O11, R= T. 
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mGURE 13; Error 2 vs. Time. Q = .O11, R = TI. 
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TTI. FUTURE FILTER IMPROVEMENTS 


The filter, as designed, will process by off-line (forward) filtering 
of the range measurements. It is suggested that, as an effort to further 
improve upon the quality of the processed data, a fixed-interval smoothing 
algorithm (the initial and final times, 0 and T, are fixed, and the 
estimate X (t/T) is sought) be incorporated. 

For the system and measurements described by: 


x = Fx + Gw 


IN 


Sik Me 


the equations defining the forward filter are, in the time domain [Ref.3]: 











A _ a Teas Sane 
eS En Re 2 eile XS Xe (1) 
ES. T i sl “ 
P= FP +PF +6QG -PHR HP, P(0) =P. (2) 
, , poe dx _ -dx 
To write the backward filter equations, set’! = 7 - t. Then as eee 
and 
< = -Fx -Gw, forO<T<T, denoting differentiation with 
respect to backward time. 
MUS. 
Ae) = eV 


Then, by analogy, the backward filter equations can be written by 


changing F to -F and G to -G, resulting in: 


d a. eal ~ 
eRe eS ig CaS 
d T i T,-1 
and a eae geen UCC rH ROH, (3) 


Sz 





FIGURE 14: Relationship of Forward and Backward Filters 


backward filter Op 
—p* —b 
0 ae t T 


forward filter 


From Figure 14, it can be seen that the smoothed estimate at time=T 


must be the same as the forward filter estimate at that point, i.e., 


u" 
><> 
—, 
—| 
ee 


X (T/T) 


and P (T/T) 


rT 
|-o 
=, 
4 

eee 


which yields the required boundary condition on a 


1 (20) = 0 (4) 


mien. : 
Eeemt=) = Cleon Be 


with the boundary condition on x (T) not yet known. Therefore, define 
the new variable: 
a A 
s(t) = Pr’ (t) R(t) (5) 


and since % (T) is finite, it follows that: 


s (t=T) = 0, ors (T#0) = 0. (6) 
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Peaomiulation in terms of Pr yields: 


d -1 _ =] d =1 
ar = (f= Py) Ps 


Thus, equation (3) can be written as: 


d -1 “le, -! p-l 


—— | | = 


ee te 
tie = 7 eS (7) 


Fp - Pe GQG' pt +H RH 


for which equation (4) is the appropriate boundary condition. 


Differentiating equation (5) with respect to T, and with some 


substitution and manipulation, we. arrive at: 


De 
[70 


d ese -1 it Tees 
tees! = lie SO Gales Z (8) 


for which equation (6) is the appropriate boundary condition. Equations 
(1), (2), (7) and (8), along with: 

ey) >= 
x (t/T) = P (t/T) (PT) (t) V(t) + PPP (t) B (t)1 


define the optimal smoother. 


Many forms of the smoothing equations may be derived. The form 
proposed for use in this particular case is the Rauch-Tung-Striebel form, 


with the discrete-time expressions summarized as follows: 
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A ai 
Ss x{ 


Smoothed State Estimate “x(k/N) = x(k/k) A, Cx( (k+1/N) - X(k+1/k)] 


where 


AL = P(k/k) O(k)! P(k+1/k) 


for k = Nel 


T 


Error Covariance P(k/N) = P(k/k) + Ay [P(k+1/N) - P(k+1/k)] Ay 


Matrix Propagation 


also for k = Nel 


Solution of the equations would proceed as follows: As an example, 
and because it is slightly easier to see when actual times are used, 
suppose NN = 100. On the forward filter pass, the values of K(k/k) 5 
Vk/k-1), P(k/k) and P(k/k-1) would be computed and stored. On the 


final iteration of the forward pass, with K = NN = 100, 
(100/100) = %(100/99) + G(100) [z(100) - H *{100/99)} 
i.e., we have computed and stored X( 100/100). 


Now, the smoothing process commences in the reverse direction. 


Decrement k to k = NN-l = 99, then 


*(99/100) =*8(99/99) + A(99) [X(100/100) - %(100/99)] 
Reseeeng yeaa? ee gece? a 


stored stored stored 
S it -] 
and A(99) = P(99/99) Q P(100/99) 
wee EE ee 


stored stored 


S5 


let k = NN-2 = 98, then 


%(98/100) = X(98/98) + A(98) [X(99/100) - X(99/98)] 
cone yar a gE ae ey ao 


stored computed stored 
last 
iteration 
E i -] 
and A(98) = P(98/98) g P(99/98) 
al 
stored stored 


Also, for each of the two preceding iterations, 


P(99/100) = P(99/99) + A(99) [P(100/100) - P(100/99)] A'(99) 
ee Ee” ara area 
Stored computed stored stored 
P(98/100) = P(98/98) + A(98) [P(99/100) - P(99/98)] A’ (98) 
Stored computed computed Stored computed 


etc. 


It is seen that the smoothing process does not involve the processing 
of actual measurement data. It does, however, utilize the complete 
filtering solution, and so fixed interval smoothing cannot be done real- 
time, on-line. It must be done after all the measurement data are 
collected. Consequently, computation speed will not be the most important 
factor. Storage requirements could, however, conceivably be, in that the 
quantities to be stored on the forward pass are arrays. It is seen that, 
should an exercise run in excess of 30 minutes, retention of the data at 
each mark could require in excess of 100K bytes of memory, which could 


limit the facilities upon which the processor could be utilized. 
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APPENDIX A: Processor Flowchart Main Program 


START 


Read 


NOUR RIO, WIDE B se 
NN, DT 










Write 
N, M, ND, MD, LD, 
| NN, DT 


Write 
IG ce P(0/-1), A, 


Cal] 
PHIDEL 








i 





Read 


Do 10 k=0, 20 


Call 
GAIN 


|= 
w 
| 

me | 


Write 
Ks G(k), P(k/k-1) 


Do 15 k=0, NN 


Call IOW 
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es 





Decode 
Dtimesez(2,1),-z2(1,1) 


















mets) = z(1,1) 
mi2.1) = = 1) 
) = 
) = 





Call PROD 
x(k/k=1) = Qx(k-1/k-1) 






mis, l 
x(4,1 





Call PROD 
zCOR = H x(k/k-1) 


Call SUB 
ZMOD = z-zCOR 
= Z-H_ x(k/k-1) 





RES1 = Z1_4(k) = xq (k/k=1) 
RES2 = Zo 4k) 7 Xo 4 (k/k-1) 
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ARES1 = /RES1/ 
/RES2/ 


ARES2 





© 


qi 
(94) x(k/k) = x(k/K-1) 


Call PROD 


xCOR = G zMOD 


= G(z2-H x(k/k-1) 


—~ =e =e oe 
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Call ADD 
x(k/k) = x(k/k-1) + xCOR 
= x(k/k-1) + G@(z-H x(k/k-1)) 









Write 8 (Bulk storage) 
Itime, RES1, RES2, ERR1, ERR2 


PRINT 
ZieZo. Xie Kee esi). 
REST, RES2, ERR1, ERR2Z 


PLOT | 
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subroutine 


GAIN 





>0 







Call TRANS 
g—>9! 


Call PROD 
TEMP3 = P(k/k) 9" 





Call PROD 
TEMP4 = @ TEMP3 
= 9 P(k/k) g! 















Call ADD 
P(k/k-1) = TEMP4 + Q 
= 9 P(k/k) g' + Q 
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; 
! 
' 
I 





©) 


Call TRANS 


H—>H! 









Call PROD 
TEMP = P(k/k-1)H" 







Call PROD 
TEMP] = H TEMP 
=H P(k/k-1)H" 







Call ADD 
TEMP1 = TEMP1 +R 
=H P(k/k-1)H! + R 









Call RECIP 
TEMP2 = TEMp1~! 
=[H P(k/k-1)H" + R] ~ 


Call PROD 


G = TEMP TEMP2 


= P(k/k-1)H' | H P(k/k-1)H! +R] 7 
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Call PROD 
TEMP3 = GH 





TEMP3 = -TEMP3 
ceca 


Call ADD 
TEMPS = HI + TEMP3 
= fe Gad 








Call PROD 
P(k/k) = TEMP3 P(k/k-1) 
4 [1 - aH] P(k/k-1) 





RETURN 
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